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@ Obstacle detection system. 

@ A system (10) that incorporates inertiai sensor information into optbal flow computations to detect obstacles 
and to provide alternative navigational paths free from obstacles. The syatem <10) a ^^^^JV P^f^^^^ 
obstacle detection system that makes selective use of an active sensor (62). The actrve detectiwi f^MVP^^caliy 
utilizes a laser Passive sensor suite includes binocular stereo (54). motion stereo (56) and vanabie fields-ohview 
{57). Optical flow computations involve extraction, derotatlon and matching of interest points from sequen^a 
frarnes of imagery, for range interpolation of the sensed scene, which In turn provides obstacle information for 
purposes of safe navigation. 
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OBSTACLE DETECTIOM SYSTEM 



Field of the Invention 



The Invention pertains to an obstacle detec^on system accwding to the preamble of claim 1. 

Particularly, the invention pertains to passive obstacle detection systems which use passive (TV, FLIR) 
5 sensors and make selective use of m active (laser) sensor. 

Background of the Invention 

Detection and avoidance of obstacles are very important to the navigation of ground and air vehicles, A 

10 system which can provide autonomous obstacle detection and avoidance is needed for such vehicles. The 
development of an autonomous system can be aciiieved by the use of active sensors (mlliimeter wave 
(MMW) riadar, la^r radar), passive sensors {television or fon*vard \cto\(\r\g infrared (FLiR)). or a combination 
of active and passive s^sors. 

An active system (tVlMW or laser) requires a very specialized and expensive sensor system. The active 

76 system risks detection by tie enemy in a battle environment. Such system does not maximise usage of 
passive s^sor technology. 

Various active systems are most advantageous In certain kinds of environments. For all weather 
conditions, MMW radar is better suited than laser radar. However, for terrain following and avoidance, ^d 
obstacle detection and avoidance, laser radar is preferred because it is less susceptible to detection by the 

20 enemy and has the necessary resolution to detect wires (e.g.. a 3 miilimeter (mm) diameter wire at a 40 
meter distance), wiiile MMW radar operating at 94 gigahertz (GHz) having a wavelength about 3mm, is 
marginally satisfactory. A iaser sensor is also better tti^ a MMW sensor for detecting objects like thin wires 
at oblk^uB angles. For day/night operatlw and countermeasare resistance, both laser and MMW sensors are 
equally good. In view of the above trade-offs between h/lMW and a laser radar, a laser ranging system Is 

26 preferable. However, many laser scanners are not adequate for such systems due to their slow scan rate 
and a lack of a large field of view (needed for providing a sufficient number of alternate directions of travel 
for a vehicle when an obstacle is enciKjntered) for successful vehicle navigation. 

Compared to active systems, a passive system has the b<^eflt of covertness, dmpliclty, reduced cost 
and ease of manufacture. Obstacle detection using passive sensors permits IJie use of two fundamental 

30 techniques for ranging --binocular stereo and motion stereo {optical flow). With the binocular stereo 
technique, ranging performance Is a function of Uie sensor resolution and the lateral displacement between 
the two senses; increased displacement Increases the maximum range measurement and improves range 
resolution. For vehicles, sensor displacement is limited by the dim^sion of the vehicle. The technique of 
motion stereo utilises one sensor from which Images are collected at regularly timed intervals while the 

35 sensor is In motion. By observing the amount of motion (on an image plane) that a world point exhibits 
between frames and using knowledge of sensor motion, range to the world point can be computed. The 
resolution of molicm stereo techniques is limited only by the resolution of the sensor. 

It is the object of the present invention to devise an improved obstacle detection system of high 
performance. 

40 This object is achieved by the characterizing features of claim t Advantageous embodiments of said 
system may be taken from the dependent claims. 

Summary of tlie Invention 

45 The present invention uses an active iaser in combination with passive devices. The invention is a 
maximally passive system, called ODIN (Obstacle Detection using Inertial Navigation), for obstacle detection 
and avoidance. It fs based upon an inertial navigation sensor integrated optical flow method and a selective 
application of binocular stereo and laser radar ranging. 

The Invention addresses the prc^lem of Integrating inertial navigation sensor (INS) information with 

50 optical flow measurements made over multiple frames to compute the range to world points that lie within 
the field of view of the sensors. Context dependent scene analysis (used to characterize the Image regions) 
and multiframe flltermg (used to predict and smooth range values) provide an improved range map. The 
INS integrated motion and scene analysis leads to a robust passive ranging technique useful for obstacle 
detection and avoidance for land and air vehicle navfgatk>n. 

The obstacle detection system integrates inertia! sensor information with optical flow and image 
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characterization components to detect obstacles and provide safe path for navigation. The system includes 
the Inertial navigation sensor, the optical flow component system, the sensor suite consisting of passive and 
gKjtive sensors, the context dependent image characterization component system, qualitative scene modei, 
range calculafions and Interpoiation. 
5 The kind of inertial navigation sensor lnf<^a^on that Is used in the obstacle detection system Include 
true heading (yaw), pitch angle, roll angte> inertial vertical speed, North-South velocity and East-West 
velocity which are used in the optical flow component system and image characterization component 
system. Addltionaliy. position latitude, position Icxigltude, ground speed, true track angle, body foil rate, 
body yaw rate, body longitucfma! acceleration, body lateral acceleration, body normal acceleration, track 
angle rate, and inertial altitude can aiso be used to synchrc^lze di^nt data rates and to achieve 
Increased accuracy. 

"me technique by which Inertial navigation sensor data is used in the obstacle detection system 
includes the integraticm of inertial navigaSon sensor data wlOi the optical flow component of the system to 
obtain Instantaneous direction of v^ide heading (focus of expan^on) and to compensate the rotation (roll, 
pitch and yaw) of the current frame with respect to the i^evlous frame. This leaves only the translation 
motion between the frames which leads to the determination of range values. The technique also includes 
the integration of inertia! navigation sensor data with the context dependent image ciwacterizatlon 
component system to achieve accurate segmentation from fr^e-to-frame by compensating for rotation 
(roll, pitch and yaw) and translation between frames* 

The optical flow component system includes Interest point extraction, derotation, and matching between 
frames, computation of the focus of expansion from inertial navigation data, compotatlwi of range values to 
worid points based on a camera modei. matching of Interest points using inertial navigation data and Image 
characteristics, and filtering of range values over multiple frames to reduce noise effects and obtain 
consistent range values. 
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Brief Description of the Drawings 



Figure 1 Is a block diagram of Ihe obstacle detection and avoidance system. 
Figure 2 illustrates the major portions of the inertial sensor integrated optical flow unit 
30 Figure 3 shows a three-'dlmenslonal coordinates system in conjunction with a two-dimensic»ial image 
plane and its own coordinate system. 

Rgure 4a illustrates overlapping fields-of-view for several types of sensing. 

Figure 4b ^ows the ^ze of sensing field of view which is required to detect the tickler sized obstacles 
for a given range. 

SB Figure 4c reveals the fl^d of view and beam coverage of a laser beam at a given range. 

Figure 5 shows the sensor geometry for two perspective views of the scene at two positions separated 
by a given distance. 

Figures 6a and 6b reveal two ways of computing the distances of Interest or world points from the ^ocus 

of expansion. ^ . 

40 Figure 7 reveals the geometry for calculating the range fi-om an Interest or world points viewed from two 

different frames of imagery. * r 

Figure 8 reveals an alternate approach for range calculation from an Interest or world point from two 

frames of imagery. 

Figures 9a, b, c and d show optical flow results of synthetic data for the invention. 
45 Figures 1 0a, h c and d show optical flow results using real data for the invention. 
Figure 11 reveals the hardware system for data coHection for the invention. 
Figure 12 reveals a computer implementation of the invention. 



Brief Description of the Tables 



Table 1 provides the parameters of the sensor suite of the inventiexi. 
Table 2 gives the coordinates of synthetic interest or world points used in the applicaSon of the 

invention. , 

Table 3 indicates the location, row. pitch, and yaw of the camera or sensor for two synthetic frames 

S6 used in tiie application of the inxrentron. 

Table 4 reveals the location, row, pitch, and yaw of the camera or sensor for two frames of real Imagery 

in the invention. 
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D^crlption ^ the Preferred Embodiments 

The preserrt invention, c^stacle detecticm and avoidance system 10, is maximally passive in that it 
combines data obtained from an Inertial navigation sensor (INS) 26 with optical flow computations In inertial 

5 sensor integrated optical flow unit 18 of figure 1, The use of the INS data permits an accurate computation 
of range to each world point based solely upon the movement (between frames) of each world point*s 
projection onto «ie Image plane. Figure 1 Illustrates inerttai sensors integrated optical flow unit 18 and 
scene analysis unit 20. Incorporating context d^erKient image charact^lzaticm and recognition of its 
components unit 12, range prediction and smoothing (using multiple frames) unit 14 qualitative sc©rje 

10 model and range calculations (to Image pixels/symbols) unit 16. using selective application 24 of binocular 
stereo 54 (passive), laser radar 52 (active) ranging, motion stereo 56 and variable fields of view 57. The 
output is from range inteipolaUon unit 28 which Is connected to unit 16. 

The incorporation of Iner^al data from unit 26 into motion st^eo 56 of unit 18 provides a robust 
approach. Traditional techniques suffer greatly from errors in the estimation of the location of the focus of 

?6 expansion (FOE) and from errors in matching world points between frames. Inertial data enable unit 18 to 
compute the exact location of the FOE and remove the effect that sensor motion (i.e., roll, pitch and yaw) 
has upon tiie imagery; thus, the motion is effectively reduced to pure translation. When the motion consists 
solely of translation, the task of world point matching is greatly simplified. The result is that more world 
points are assigned matches from frame to fr^e and that the range measurements have improved 

20 accuracy. 

For a pair of image frames, the major steps of optica! flow method 30 as siiown in figure 2 begin with 
input framesi frame N-1 and frame N whidi are digitized video or FUR images, being read in from passive 
sensors 22 along with units 32 and 34 wherein Interest points ere extracted from input frames N-1 and N. 
The extracted interest points are sent to interest point matcher 38 and interest point derotation unit 40, 

2d respectively. Location of the focus of expansion (FOE) (in both frames N-^l and N) is computed In FOE 
computational unit 36. Computational unit 36 output goes to Interest point matcher 38. Inertiai measurement 
unit 26 outputs rotational velocity informaUon to derotational unit 40 and translational velocity information to 
FOE computational unit 36. range to interest points unit 42 and range Interpolation over the entire area unit 
44. FOE and ih& Interest points in frame N are projected onto an image plane that Is parallel to the image 

30 plane that captured frame N-^l (derotation of frame N). interest points in frame N are matched to those of 
frame N-1 based upon four criteria. Range is computed to each interest point in frame N that has a match 
In frame N-1, A dense range map is created using context dependent scene analysis and interpolating 
between the computed range values. 

The Imagery to system 30 is digitized and contains pixels addressed by row and column with the origin 

35 of 2-D coordinate system 48 of figure 3 located in the upper left corner of the image. The horizontal axis, c, 
points to the right and the vertical axis, r, is in tiie downward direction. This image plane 46 is perpendicular 
to the X axis of 3-D coordinate system 50 and is located at a distance of the foca! length F from the origin 
with itie z axis in the dovimward direction. Therefore, the pixels in image plar^ 46 can be described in 2-D 
coordinate frame 48 as (c. r) and in 3-D coordinate frame 50 by the vector (F, y, z). The gwmetry 

40 described above is graphically iHustrated In Figure 3. 

As shown in Figure 2, the data input to the obstacle detection method 30 consists of a sequ^e of 
digitized video or FUR frames that are accompanied by inertial data consisting of rotational and translational 
velocities. This information, coupled with the temporal sampling Interval between frames, is used to 
compute tie distance vector, rf» between each pair of fr^es and the roil, pitch and yaw angles, (^. e, \t^), 

46 of each frame. Both J and (<;». 9, ^) are crucial to method 30. 

The movement of the world points* (i.e., interest points') perspective projection (onto the image plane 
46) is at a minimum near the FOE and, as a result, the range to the world or interest points nearest the FOE 
have the greatest amount of uncertainty associated with them. The passive ranging technique of binocular 
stereo 64 is most accurate near the center of the field of view (where the FOE is located most of the time) 

50 and is less accurate near the edges of the field of view. In addition, the binocular stereo 64 approach can 
function even when the vehicle is stopped or hovering. 

Wires and other small obstacles are detected by active sensor 52 and passive techniques 54 and 56 
because the greater resolution 57 (Figure 1) and 64 (Figure 4a) required to detect such obstacles at a 
range sufficient for obstacle avoidance. A tradeoff Is made between the field of view and resolution of the 

55 s0nsor(s). Since the system's field of view must be large enough such that the vehicle has suffldent 
(previously scanned) directions In which to steer when obstacles are detected, the field of view of the 
passive sensors can not be reduced; hence, laser range scanner 52 and a narrow FOV passive sensor 
function in conjunction with passive sensors 54 and 56 of system 10. The use of a simple (i.e.r circular 
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scanning) laser range sensor 52. whose scan pattern is centered around the FOE. Is for the purpose of 
detectina only small obstacles that lie within the vehicie's direction of travel, 

An illustration of the overlapping fields of view 58. 60 and 62, respectively, of the three types of sensing 
(optica! flow 56. binocular stereo 54, and laser sensor 52) is in Rgure 4a. A combination of these types of 
B sensors yields a robust obstacle detection and avoidance system. Laser sensor 52 provides sufficiently high 
resolution not provided by passive means 54 and 56. Limited field of view 62 of laser beam sensor 52 
sacrifices Uttte covertness. and the simplicity of its scanning pattern keeps acquisition time short and 
hardware complexity low. GImbated laser scanner 52 can also be used to quickly investigate avenues of 
safe passage when obstacles have been encountered which block the cunrent vehicle paih. Multipurpose 
10 passive sensor FOV 64 encompasses laser sensor FOV 62. , , . . 

Raure 4b illustrates the size of sensor FOV 66 which is required to detect obstacles at a range of 40 
meters in the flight path of rotorcraft 6S. In figure 4c. a 0.5 milliradian (mrad) laser beam width having FOV 
62 scans in circular pattern 70. Step size 8 used in scanning is 0.70 mrad (in the plane of scan circle 70). 
leading to 8,900 range samples in 2 p radians, At a range of 40 meters, the laser beam is 2 cm in diameter 
,6 which leads to an overlap of the beam between samples. This overiap and small ^teP S re^J-" ^ 
range samples being acquired, at the range of 40 meters, on a wire of 3 mrn f^'^^^ .^^^J^ 
perpendicular to the tangent to scanning circle 70. Laser range samples yield °f ^he 
range values are compared to a minimum acceptable range threshold. World pomts hav.ng a range less 

than this threshold are a danger to the v^icle. _ 

Table 1 lists sensor types and typical parameters for FOV. array size, instantaneous FOV. and 



Each sensor and its function may be described, with emphasis on obstacle detection and avoidance m 
the context of rotorcraft navigation. First note that there are two types of sensor mountings - those of 
gimbal controlled orientation and those of fixed orientation. 

s Motion stereo sensor 56. which is of fixed orientation, is used to generate a sparse range map of wo Id 
features over FOV 58. The wide FOV 58 Is required for the sparse range map to provide suitable options for 
rotoiwaft 68 maneuvering when an obstacle is encountered. Binocular stereo sensor 54 is used to provide 
range measurements over a mecSum FOV 60 that is centered within wide FOV 58 of motion stereo sensor 
56. The purpose of binocular steiBO sensor 54 is to provide range samples 

so stereo 56 measurements are the most error prone, around the Instantaneous directfon of vehicle heading 
(i.e.. focus of expansion), which ties mainly within the center of FOV 58. In addition the binocular stereo 54 
measurements can be made when a vehicle is stationary (e.g.. when rotorcraft 68 ,s hovering or when it is 
turning about its vertical axis without forward motion), thereby providing a range ">aP JJ-^'^h can be used o 
perform obstacle detection. Both the binocular 54 and motion 56 stereo sensors use TV or FUR imagery to 

36 perform these measurements for day and night operations. 

Two kinds of sensors are mounted on a gimbaled platform -a variable FOV passive .sen^J. V or 
aiB) 22 and a scanning laser range finder 52. Placing the sensors on a gimbal allows their FOVs to be 
constantly focussed in the direction of rotorcraft travel, which is necessary since the sensors must be able 
to detect large and.small obstacles (such as wires) which fle in the immediate path of tiie rotorcraft. Laser 

« range finder S2 actively scans for obstacles and the passive sensor data are used to perform mo^onf^f^ 
S^Lsurements or to simply extract two-dimensional (2-0) features which have a high probability of being 

obstacles (e.g., linew features of poles, wires, etc.). ^„,,„,to 
An additional benefit of having gimbaled sensors Is that sensors' FOVs can be directed to an aUemate 
flight corridor when obstacles are detected in the current corridor of the vehicle. Turning the sensors to he 

45 alternate corridor is necessary to determine the suitability of the corridor prKjr to execuhng a change in *e 
fflsht path as part of the obstacle avoidance task. The alternate flight corridors are determined from «>e 
rS^ge measurements made by wide FOV fixed position sensors 56 and 54. in addition, the gimbaled 
sei^re can be directed on a potential landing site for the purpose of obstacle detection prior to iandmg. In 
the air vehicle scenario, the gimbaled sensors may also be controlled by helmet mounted sensors^ 

so V\fide FOV 58 of motion stereo sensor 56 is chosen to provide a wide, cleared area in which a lateral 
maneuver may be performed if an obstacle is defected. The vertical FOV is half of the horizontal FOV due 
iD the nature of nap-of-the-Earth flight (in the air vehicle rotorcraft scenario) in which vertical maneuvers are 
not desired and lateral maneuvers are emphasized. BInccuiar stereo sensor 54 has a smaller, more 
conventional FOV 60 which is centered within motion stereo FOV 58 to compensate for range measurement 

56 error that occurs near the FOE in motion stereo measurements and 1» provide range measurements when 
the rotorcraft is not undergoing forward translation, .r *i, irr,c ...uh hi«h 

The flimbaled sensors are designed to track the FOE of vehicle motion. Tracking the FOE with the high 
resolution passive and laser sensore provides the most accurate ranging techniques where they are needed 
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most. The FOE is not the only location that needs to be sensed by the gimbaled sensors. To perform 
obstacle avoidance, i.e-, to select a new flight corridor, and to 'clear' a ground bcadon prior to landing, the 
gtmbaled sensors must be directed automatically by obstacle detection signals from unit 44 of figure 2, or 
manually by the ptlot or co-pilot User 52 detection of an obstacle car* be confirmed with data from the 

5 passive, gimbaied sensor whose \\m of sight Is parallel with that of the laser sensor. 

Once range samples are obtained from the various sensors, the next step involves obstacle detection to 
be followed by obstacle avoidance. This requires that the computed range for the scene be sufficiently 
dense (so as to extract the discontinuities in the range map; ^ese discon^nultles correspond to the 
presence of obstacles) or a model for the scene be available. A model means a segmentation of the sensed 

w image in which the various segments are labeled as to their respective types of terrain {sky. road, grass, 
etc.)* Context dependent image characterization, also called "scene analysis," is applied to each frame, 
resul^ng in a model of the scene which aids In the identification of safe paths and aids the process of 
Increasing the density of the range map. 

Interpolation of the range values obtained by the optical flow metiod of system 30 of figure 2, is aided 

?5 by results of scene analyses. Having information about the scene allows for intelligent interpolation between 
the range values. For example, when range values fall on two trees separated by 25 meters of unobstructed 
space, the scene model can be used to prevent the r^ge Interpolation from connecting the trees and 
blocking off the space between them. The result is a more dense and accurate range map which can 
subsequently be used for obstacle avoidance. 

20 The features within the Imagery (TV or FLIR) that are most prominent and distinguished, mark the world 
points to which range measurements will be made. These prominent world points, known as interest points, 
are easy to extract from the imagery and have tiie highest promise of repeated extraction throughout 
multiple frames. The interest points within the fleld-of-vlew of the monocular sensor are of fundamental and 
critical importance to optical flow calculations. The extraction and subsequent use of Interest points are 

25 described below. 

Interest point selection involves compulation of distinguishable points which is accomplished by passing 
a Moravec operator over each frame of Imagery. The operator is applied to each image pixel {within a 
desired offset from the Image border) which was idendfied as a strong edge pixel by a Sob^ edge operator. 
The interest operator examines all pixels within a square window, of side length L, that surrwnds each edge 

50 pixel and computes the relative variance between pixel values. As each pixel within the window is 
examined, the square of the difference between its value and the values of its neighboring pixels is 
computed and summed- Actually, four different sums are recorded which correspond to the same four 
neighbors relative to each pixel within the window; itiere Is a sum for the square of the difference between 
the current pixel and Its neighbor to the right and likewise for ttree other neighbors (below, below & right, 

3$ below 8c left)- After each pixel under the window has contributed to the four sums, the smallest of the sums, 
S, is selected and stored as the pixel's value, A pixel is deemed an interest point if its assigned value of S 
is greater to the corresponding sum generated at each pixel within a square window of side length K 
centred on the pixel In question. In the discussion that follows, a pixel's value of S will be referred to as its 
Interestlngness, 

40 Implementation of the Moravec operator ranks the detected interest points (pixels with a value of S 
which Is a local maximum) in the order of their computed interestlngness. This interest point extraction 
routine woi1<s on the segmented image obtained by context dependent scene analysis 12 (Figure 1). 
Segmentation divides the image Into M uniform regions. Interest pdnt routine returns only the N points 
within each region which have the highest values of S, where N and M are inputs to the program. The result 

45 of returning only the best interest points (in terms of 8) in each region is that the processed scene is 
intelligently covered with interest points. 

If this were not the case, a small number of occasionally adjacent regions will lay claim to the major portion 
of Interest points. 

Note that not all regions within a scene can contain reliable Interest points (e,g., wave crests on a body 
50 of water are not good Interest points). As mentioned above, image charactwizatlon 12 Is used to ascertain 
the goodness of regions prior to interest point selection. Interest point seiection can be further Improved by 
incorporation of Kalman filtering techniques, which use inertial sensor data to track and predict Interesting 
point features. 

Interest point derotation aids the process of interest point matching. One must make it seem as ttioi^h 
55 Image plane B is parallel to image plane A. If this is done, the FOE and pairs of interest points in tames A 
and B that match, would ideally be coflnear should the image planes be superimposed (see Figure 5), 
Rgure 5 Is an illustration of the sensor geometry that records two perspective views of a scene at two 
positions separated by a distance ^ |At = fj j (with no rotation of the sensor between positions). When there 
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is no rotafional change between image frames, there is a special property of the perspective projection of a 
world point onto the two image pianes; FOE and the projections of the world point are ail colniear. 

To make the image pianes paraHel, derotatlon fs p^mied for each vector, (F.yi^zi), that corresponds to 
each Interest point in frame B. The equation for ttie derotatlon transformation and projection (m homo- 
geneous coordinates) is: 
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and where NED (north, east, down) Is ttie coordinate frame In which inertial measurements are made. Use 
of the NED frame assumes that vehicle motion is "local" to a patch of Ear*). 

The matrix P projects a world point onto an image plane and is used to compute the FOE. FOE = P d, 
where d = wt. The matrix C^ned converts points described in the NED coordinate frame into an equivalent 
descriptton wtthin a coordinate frame parallel to me A coordinate frame. Utewise, the matrix Cb converts 
the descriptions of points in the B coordinate frame Into descriptions in a coordinate frame parallel to NED. 

The matching of interest points is performed in tvro passes. The goal of the first pass is to Identify and 
Store the top three candidate matches for each Interest point in frame B. (F, ysj' 'Bj). 
The second pass looks for multiple Interest points being matdied to a single point in frame A. Hence, the 
resuit of the second pass is a one-to-one maich between the interest points in the two successive frames. 
For the present embodiment, a one-to-one match of interest points is necessary, The projection onto the 
sensor's image plane of an obiect in the world will grow in size as the sensor moves toward the object. This 
situation might imply that a one-to-one match is nonsensical since what was one pixel in size in frame A 
might become two or more pixels in size in frame B. It is assumed that the growth of objects, in terms of 
pixel size, is negliglWe in the passive ranging for obstacle detection scenario. Ail objects are assumed to be 
at certain safe distances for vehicle maneuvering and one pixel (of interest point quality) in two franes is all 
that is required of an object's surface for the range to the object to be computed. 

The first pass is described in the following. To determine the candidate matches to (F.yej, zb)), each ot 
the interest points in frame A is examined with the successive use of four metrics. The first metric mal«s 
certain that candidate matches lie within a cone shaped region bisected by the line joining the FOE and the 
interest point in frame B. This metric HmH» candidate matches to lie within the cone with apex at the FOE. 
as shown in Figure 6(a). If an Interest point In frame A. (F.yAi, za,). passes the first metric, then the second 
metric Is applied to it. The second metric requires that the interestlngness of candidate, matehes is dose to 
the interestlngness of the point that we are trying to mat*. (Figures 6a and 6b show constraints used to ad 
the process of matching interest points between frames.) 

The third metric restricts all candidate matehes In frame A to lie closer to the FOE than the points of 
frame B (as physical laws wotild predict for stetionary objects). This metric involves «ie computation of the 
distances of the interest points from the FOE. which can be computed in two different ways. Tlis ftrst is the 
direct euclidean distance. di. between {F,yA„ zai) and (F.yej. zs)), and the second is the distance ds which is 
the projection of d, onto the line joining (F.ys,. zb,) and the FOE. The distance measures are graphically 
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illustrated m Figure 6{fci). Regardless of the way that the distance measure Is computed, it car^ be used to 
identify closest candidate matches to (Rysj. z^}. 

The fourth metric constrarns the distance between an interest point and Its candidate matches. For an 
Interest point in frame A, Aj, to be a candidate match to pdnt Bj, it must lie within the shaded region of 
Figure 6(a). The depth of the region is determined by this fourth metric while the width of the region Is fixed 
by an earlier metric. By limiting interest points. Aj, to lie in the shaded region, one has effectively restricted 
the computed rang© of resulting matches to lie between R,T>ax snd B^in- The reasoning behind this 
restriction is that world objects of range less than Rmfe» not occur due to autonomous or manual 
navigation of the vehicle, thus avoiding potential cdlisions. Likewise, objects at a range greater than Rmax 
are not yet of concern to the vehicle. 

The result of the first pass of interest point matching Is a list, for each (F.ysi, zbj), of three or fewer 
candidate matches that pass ail metrics and have the smallest distance measures of all possible matches. 

The goal of the second pass of the mafching process Is to \dke the matches provided by the first pass 
and generate a one-to-one mapping between the Interest points In frames A and S, Initially, it can be 
assumed that the best match to (F,yB], Zbj) will be the stored candidate match which has the smallest 
distance measure. However, there may be multiple points, (F.yBj. zsj), which match to a single {F.yAi, zaO- 
Hence, the recorded list of best matches is searched for multiple occurrences of any of the Interest points 
in frame A. Jf multiple interest points in frame B have the same best matdi, ttien the point, B\ which is at 
the minimum distance from the A| in question, will retain this match and Is removed from the matching 
process. The remaining Bys are returned to the matching process for further investigation after having A? 
removed from their lists of best matches. This process continues until all of the Interest points in frame B 
elBier have a match, or are (fetermined to be unmatchable by virtue of an empty candidate match list. Thus* 
the final result of the matching process is a one-to-one mapping betv^n the interest points in frames A 
and B, 

Given the result of interest point matching, which is the optical flow, range can be computed to each 
match. Given these sparse range measurements, a range or obstacle map can be constructed. The 
obstacle map can take many fomis, the simplest of which consists of a display of bearing versus range. 
The ne)^ st^ is range caloulaUon and interpolation. 

Given pairs of Interest point matches between two successive Image frames and translational 
velocity between frames, it becomes possible to compute the rang© to the object on which the interest 
points lie. One approach to range, Rt computation is described by the equation 



where 

Xf = the distance between the FOE and the center of the image plane, 
x~ the distance between the pixel In frame A and the center of the image plane, 
X' - the distance between the pixel in frame B and the c^fiter of the image plane, 
AZ= P I At cos« - the distance traversed in one frame time, At. as measured alone the axis of the 
line of sight. 

o= the angte between the velocity vector and the line of sight, 

x'- Xi the distance in the image plane between (F,ye].3:Bj) and the FOE, and 

X' - x» the distance In the Image plane between (F,yaj,2:e|) and (F,yAi,ZAi) 

These variables are illustrated In Figure 7, wherein the geometry involved In the iirst appro^h to range 
calculation is also illustrated. Figure 7 shows the imaged world point in motion rather than the sensor, 
hereby simplifying the geometry. 

An alternate approach Involves the calculation of the angles ba and as between the fe-anslatlonal velocity 
vector and the vectors that describe the matched pair of interest points In frames A and 8, 



R K IvlAt sing 

as indicated in Figure 8, wherein range calculation requires the computation of angles between the linear 
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velocity vector and the vectors that describe the matched pair of interest points. Both of the rarjge 
calculattng techniques compute the disfartce to a world point relative to the lens center of frarne A {similar 
equations would compute the distance from the lens center of frame B). The accuracy of the range 
measurements that result from either approach is very sensitive to the accuracy of the matching process as 

B w^l as the accuracy of the Inertial measurement unit (IMU) data. 

The tas(« of range interpolation is the last processing step required in the passive ranging system 
(excluding any postprocessing of the range that may b& required before the processed data reaches the 
automatic vehicle control and display systems). By means of rat^e interpolation between the sparse range 
samples generated from the optical flow measurements, a dense range map representing the objects wrthin 

,0 the field of view is established. Essentially, this task is surface fitting to a sparse, nonuniform set of data 
points. To obtain an accurate surface fit that physically corresponds to the scene within the fteid of view, tt 
is necessary that the sparse set of range samples correspond to the results obtained from scene analysis. 
As mentioned above, image segmentation, context dependent image characterizations and recognition of its 
components 12 (Figure 1) are used to create regions from which a desired number of interest p«nts are 

,s extracted^ ^^^^ significant because the resulting surface (i.e., the range map) must 

pass through each of the range samples. It would be particularly detrimental if the surface passed under 
any range samples. Many techniques of surface fitting are applicable to ttw present task. 

One type of range interpolation consists of a fitting of planes to the available range samples. This 

20 approach accomplishes the tasl« efficiently and succeeds in passing through ea* range sample. Any 
technique of range interpolation needs to avoid interpolation over discontinuities that occur between range 
samples on the surface of concern. With scene anaiysis/segmsntation, the smoothing of discontinuities is 
avoided by inlerpotating only over smooth regions or segments of the scene. 

Range computation (based on 2 frames) Is further improved by estimaOng range over nriultlple frames. 

28 The procedure for prediction and smoothing of range using multiple frames 14 (Figure 1) Is ^ 
{merest points in a pair of images, compute matching confidence, measured and predicted ranges. 
cOTifldence in range and threshdd the result are computed to c^tain the final range. 
Matching Confidence of ith point in frame n Is given by 
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where ^2 0 and o>1 4 ^2 « 1. t,x is the Interestingaess of Ith point in frame X. di Is the projection of 
ith point (point A in Figure 6b) on the line connecting FOE with Its match (point B in Figure 6b). Range 
ccmfidenoe of ith point in frame n is given by the followmg set of equations. 
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The inertia! navigation sensor integrated optical flow method has been used to generate range samples 
u^ng both synthetic data atxi real data (imagery and INS information) obtained from a moving vehicle. In 
the following, results are illustrated using a pair of frames. Synthetic interest points were generated from a 
file contamlng the 3-0 coordinates of 15 worid points, T^le 2 shows the 3-D locations of these world 
points. In the same coordinate system as the interest points are located, table 3 lists the location, roll, pitch, 
and yaw of the camera at the two Instances of time at which synthetic frames A and B were acquired. The 
time between frame acquisition is 02 seconds. Figures 9a, b, c and d show optical fbw results of sythetic 
data. Figure 9a Indicates the k>cations (circles) of the projection of the world (or Interest) points onto the first 
location (I.e,, first Image) of the Image plane w4iere the Held of view of the synthe^zed camera model Is 
52.0 degrees x 48,75 degrees with a focal length of 9 mm. Figure 9b shows the locations (squares) of the 
projections of the world (or interest) points onto the second location (i.e., second image) of the image plane 
and shows Ihe new locations (diamonds) of those projections after derotation. Figure 9c shows the results of 
the matching process In which circles are connected to their corresponding diamond with a straight line and 
the FOE is labeled and marked with an X In other words, the matching process results in displacement 
vectors between the circles and the diamonds. The final frame, Figure 9d, shows the computed range value 
to each point resulting from each of the matches > 

A pair of real images was selected to test the capabifities of the optical flow method using real imagery. 
Tat^ 4 Indicates the location, roil, pitch, and yaw of the camera associated with the pair of reai image 
frames tiat were used. The field of view of the camera for the real images Is 62,1 degrees x 40,3 degrees 
and the focal length = 9 mm. The elapsed time between the two frames for this experiment was 0.2 
seconds. Figures 1 0a, b, c and d reveal optical flow results using real data. Figure 1 0a shows the locations 
of the extracted interest points obtained from the first frame, drawn as circles. Similarly, Figure 10b 
indicates the location of extracted interest points (squares) and the corresponding derotated interest point 
locations (diamonds). Since the vehicle und^goes very little rotation between frames, the derotated 
locations are nearly coincident with the original point locations, Ihe results (i.e., displacement vectors 
between circles and diamonds) of the point matching process for the reai imagery, with the FOE Indicated 
by an X, is shown in Figure 10c. Finally, the computed range value to each of the matched points is 
displayed in Figure 10d. 

Figures 11 and 12 show the hardware system used for data collection by a ground vehicle and the 
ODIN system implememation. Figure 11 is a diagram of hardware system SO for data collection for the 

motion field of view for obstacle detection system 10 of figure 1, VME chassis 82 (VME is an internationat 
standard bus) comprises central processing unit (CPU) 84 (model 68020) having serial port 86 and system 
clock 88 connected to CPU 84, an input/output proto board 90 connected to serial port 86 and system clocl< 
88 of CPU 84, and DalaCube (Boston, MA) hardware 92 that includes Digmax board 94 and MaxGraph 
board 96 which Is connected to system clock 88. A Honeywell model 1050 ring laser gyiroscqpe (RLG) 
rnertial reference unit (IRU) Is connected to i/0 board 90 and provides inertial data with tln^ stamping, 
which is collected at 50 Hz. Sensor 100, which is a Panasonic television camera, model WV-1850, having a 
focal length of 25 mm and an FOV of 20 degrees by 1 5 degrees, is connected to board 96 and provides 
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imagery data. Output from chassis 62 goes to Panasonic optical disk recorder 102, modei TQ-2023F. 
Reorder 102 is connected to serial port 86 and board Video frames to recorder 102 have a time stamp 
and are recorded at 5 Hz sychronously with IRU data from inertial unit 98 to chassis 82. The data In 
recorder 1 02 are for optical flow unit 18 of figure 1 . 

5 Figure 12 is hardware Implementation 110 of obstacle detection system 10 of figure 1. Computer 112 
(model Sun 3/280) receives television imagery and INS data. Data in recorder 102 of figure 11 go to disk 
114 of computer 112, Data are In the form of 500 x 480 pixel images sampled at a 5 Hz rate. The INS data 
from unit 98 {from Honeywefl 1050 RLG inertia) measurement unit (Mi))) are In terms of latitude and 
lonQltude, The parameters are: a ^ (a«, a,, a,) ft / (50 Hz): t = (Vx, Vy, v^) ft / 5(20 Hz); and t = a, 

TO ^) degrees (20 Hz), wherein j> is yaw, $ is pitch, and ^ is roll. 

Computer 12 is connected to computer 116 (model Sun 3/60) via ethernet 118, Computer 116 has a 
CPU, a math coprocessor and associated memory. It operates in G language in 4.2 BSD Unix {release 3,5) 
software. Computer 118 performs functions of system 10. parlrcularly. units 18. 20, 24 and 28 of Figure 1, 

i& Claims 

1, Obstacle detection system, charaoterized fay : 
a seen© analysis unit {20); 

an inertial sensor irrtegrated op^cai flow unit (18) connected to said scene analysis unit (20); and 
so an integrated inertial navigation unit (26) connected to said scene analysis unit (20) and to said inertial 
sensor integrated optical flow unit (18). 

2, System according to claim 1 further charaoterized by : 
at least one passive sensor (22) connected to said scene analysis unit (20); 
a selector unit (24), connected to said scene analysis unit (20) and said integrated Inenial navigation 
unit (26), having active scanning (62) binocular stereo (54), motion stereo (56). and variable Helds-of- 
vlew (67) of a scene where an obstacle may be detected and an alternate path to avoid an obstacle 
may be found vidth a selective aid of active scanning (52), binocular stereo (54). motion stereo (56) or 
var?abte flelds-of-vlew (67); and 

$0 a range interpolation unit (28), 

3, System according to claim 2, characterfeed in that said scene analysis unit comprises: 

a context dependent image characterization unit (12) connected to said passive sensor unit (22), to 
integrated inertial navigational unit (26), to said Inertial sensor integrated optical flow unit (18). and to 

$5 said selector unit (24); 

a range predictor and smoother unit (14) using muftlple frames connected to said inertial sensor 
int^rated optical flow unit (1 8) and to said integrated inertial navigational unit (26); and 
a qualitative scene rrwdel and range calculation unit (16) connected to said inertia^ sensor integrated 
optical flow unit (18), to said range Interpolation unit (28), to r^g predictor and smoother unit (14), and 

40 to said selector unit (24), 

4, System according to claim 3, charaoterii^ed in that said inertial sensor integrated optical flow unit (18) 
compMrises; 

46 a first Interest point extractor (32), connected to said scene analysis unit (20), for extracting interest 
points from a first frame of imagery; 

a second interest point extractor (34). connected to said scene analysis unit (20). for exfe-^tlng Interest 
points from a second frame of imagery; 

an interest point derotatlon unit (40) connected to s^d second interest point extractor (34) and to said 
50 Integrated Inertial navigational unit (^)j . 

a focus of expansion computational unit (36) connected to said integrated inertial navigational unit (26); 
an Interest point matching unit (38) connected to said first Interest point extractor (32), to Interest point 
derotator unit (40). to integrated inertial navigational unit (26). and to said focus of expansion 
computatlORai unit (36); and 

a range to mafched fnterest points measurement unit (42) connected to said interest points matcher 
unit (38), to integrated inertial navigation unit (26), and to said range interpofatlon unit (28). 

6, System according to claim 4, characlerfzed in that said range interpolation unit (28) outputs obstacle 
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detection and avoidance inforniatlon in the range data for tl^e scene. 
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SENSOR TYPE FIELD OF 

VIEW 


ARRAY SIZE 
(PIXEIS) 


INSTANTANEOUS 
FOV (mrads) 


RESOLUTION 
(cm AT 40ni) 


MOTION STEREO 120*)(60* 


512x256 


4.09 


16.36 


BIN0CUU4R STEREO 40*x20» 


512x256 


1.36 


5.45 


MULTI-PURPOSE 26*x25* WIDE 

0.72'xO J2* NARROW 


512x512 
512x512 


0.852 

0.025 


3.41 
0.1 


LASER RANGE CENTRAL 

ANGLE « 20* 


CIRCULAR 
SCAN 


0.5 


2.0 



Table 1 





x (ft) 




zin) 


1 


100 


25 


4 




95 




4 


3 


90 


-10 


4 


4 


85 


-5 


4 


5 


80 


2 


4 


6 


75 


8 


4 


7 


70 


"*8 
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65 


10 
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60 
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55 
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11 


50 


-15 
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12 


35 


10 
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13 


30 
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14 


25 


-6 


4 


15 


20 


2 


4 



Table 2 





x(ft) 




z(ft) 


ROLL (deg) 


PfTCH (deg) 


YAW (deg) 


FRAME A 


0 


0 


-7 


0 


—15 


0 


FRMrfEB 


5 


1 


-6 


5 


-11 
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Table 3 




x(ft) 


y (ft) 


z(n) 


R0a(d«9) 


PTFOH (deg) 


YAW (deg) 




-230.3 


-20.72 


6.43 


0.959 


-1.179 


-176.737 


FRAME B 


-231.7 


-20.83 


6*44 


1.222 


-1.231 


-176.852 



Table 4 
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